#include "modbus.hpp"
/**
 * @brief convert frequency level to real frequency value(Hz), be wary, this returns 0 if
 * level is 0
 *
 * @param level 0, 1, 2, 3, 4
 * @return int 0, 5, 10, 20 or 30, -1: exceed range
 */
int level_to_Hz(int level)
{
	static const int hz[5] = {0, 5, 10, 20, 30};
	if (level < 0 || level > 4) return -1;
	return hz[level];
}

/**
 * @brief Construct a new Modbus_controller object and config tty port to 115200bps, 8 bit,
 * no parity, 1 stop
 *
 * @param filep tty device file descriptor
 * @param addr initial target device address(from 1 to 247, 0 for boardcast) used by all
 * read_xxx() and write_xxx() methods, if you're not sure about the device's address, use 0
 * for broadcast and call read_address() to get its address, then call set_address() to
 * update this buffered value
 */
Modbus_controller::Modbus_controller(int filep, uint8_t addr, std::string sem_name)
{
	struct termios attr;
	this->fp = filep;
	this->addr = addr;
	this->file_lock = std::make_shared<plock>(sem_name);
	// config tty, disable all unnecessary features
	tcgetattr(this->fp, &attr); // get terminal attributes
	cfmakeraw(&attr);			// set to raw mode, all inputs will be treated as raw byte data
	cfsetspeed(&attr, B115200); // set baud rate to 115200
	attr.c_cc[VMIN] = 0; // VMIN, read() is able to return even if no charater was received
						 // if VMIN was set to 0
	attr.c_cc[VTIME] =
		1; // VTIME, read() will wait for at most VTIME*100ms if no character was received
	attr.c_cflag &= ~(PARENB | CSIZE);
	attr.c_cflag |= CS8;				 // disable parity check and set byte size to 8 bits
	tcsetattr(this->fp, TCSANOW, &attr); // set terminal attributes
}

/**
 * @brief calculate crc checksum for given bytes
 *
 * @param dat data buffer
 * @param len data length in bytes
 * @return uint16_t crc checksum
 */
uint16_t Modbus_controller::get_crc(unsigned char *dat, int len)
{
	uint16_t crc = 0xFFFF;
	int i, j;
	for (j = 0; j < len; j++)
	{
		crc ^= dat[j];
		for (i = 0; i < 8; i++)
		{
			if ((crc & 0x01) > 0)
			{
				crc >>= 1;
				crc ^= 0xA001;
			}
			else
			{
				crc >>= 1;
			}
		}
	}
	return crc;
}

/**
 * @brief capture a raw datagram
 *
 * @param datbuf data buffer
 * @param buflen size of the buffer
 * @return int 0: no valid datagram, -1: error when read tty, -3: no enough buffer space, -4:
 * cannot get lock
 */
int Modbus_controller::read_raw(uint8_t *datbuf, int buflen)
{
	uint16_t crc;
	int err;
	uint8_t dat[256];
	// try lock, return if timeout
	err = 0;
	while (!err)
		err = this->tty_lock.try_lock_for(std::chrono::milliseconds(5));
	if (!err)
	{
		printf("try lock timeout when read regs, nested call is not allowed!");
		return -4;
	}

	if (this->file_lock->trylock_for(5000) < 0)
	{
		printf("unable to access tty file, semaphore locked by another process");
		this->tty_lock.unlock();
		return -4;
	}

	do
	{
		// read
		err = read(this->fp, dat, 256);
		if (err < 0)
		{
			perror("failed to read tty");
			err = -1;
			break;
		}

		// check size
		if (err > buflen)
		{
			err = -3;
			break;
		}

		// check address
		if (dat[0] != this->addr)
		{
			err = 0;
			break;
		}

		// check crc
		crc = uint8_to_uint16(dat[err - 1], dat[err - 2]);
		if (crc != this->get_crc(dat, err - 2))
		{
			err = 0;
			break;
		}

		// valid, copy to user
		memcpy(datbuf, dat, err);
	} while (0);

	this->file_lock->unlock();
	this->tty_lock.unlock();
	return err;
}

/**
 * @brief read multiple words from register
 *
 * @param reg register address
 * @param cnt how many words are expected(16 bits)
 * @param datbuf output buffer
 * @param buflen buffer length(in byte)
 * @return int -1: error when read or write tty, -2: bad recv crc or length, -3: no enough
 * buffer space -4: locked, >0: how many bytes were successfully read
 */
int Modbus_controller::read_regs(uint16_t reg, uint16_t cnt, uint8_t *datbuf, int buflen)
{
	uint8_t buf[8];
	uint16_t crc;
	int err;
	uint8_t dat[256];
	if (buflen < cnt * 2)
	{
		return -3;
	}
	// try lock, return if timeout
	err = 0;
	while (!err)
		err = this->tty_lock.try_lock_for(std::chrono::milliseconds(5));
	if (!err)
	{
		printf("try lock timeout when read regs, nested call is not allowed!");
		return -4;
	}

	if (this->file_lock->trylock_for(5000) < 0)
	{
		printf("unable to access tty file, semaphore locked by another process");
		this->tty_lock.unlock();
		return -4;
	}

	// if error happened, break and unlock mutex
	do
	{

		buf[0] = this->addr;
		buf[1] = 0x03;
		buf[2] = (reg >> 8) & 0x00FF;
		buf[3] = reg & 0x00FF;
		buf[4] = (cnt >> 8) & 0x00FF;
		buf[5] = cnt & 0x00FF;
		crc = this->get_crc(buf, 6);
		// crc low byte first
		buf[6] = crc & 0x00FF;
		buf[7] = (crc >> 8) & 0x00FF;

		err = write(this->fp, buf, 8);
		if (err < 0)
		{
			perror("failed to write tty");
			err = -1;
			break;
		}
		// according to the manual, must delay at least 3.5 characters between two packages,
		// 300us should be enough
		usleep(1000);
		// err = read(this->fp, dat, 5 + cnt * 2);
		err = read(this->fp, dat, 256);
		if (err < 0)
		{
			perror("failed to read tty");
			err = -1;
			break;
		}
		// crc low byte first
		crc = uint8_to_uint16(dat[4 + cnt * 2], dat[3 + cnt * 2]);
		if (crc != this->get_crc(dat, 3 + cnt * 2) || dat[2] != cnt * 2)
		{

			printf("\r\nrecv: ");
			for (int i = 0; i < cnt; i++)
			{
				printf("%02x ", dat[i]);
			}
			printf("\r\n");
			err = -2;
			break;
		}
		memcpy(datbuf, dat + 3, dat[2]);
		err = dat[2];
	} while (0);
	this->file_lock->unlock();
	this->tty_lock.unlock();
	usleep(300);
	return err;
}

/**
 * @brief write one word to a register
 *
 * @param reg register address
 * @param dat data to be writeen
 * @return int 0: success, -1: error when read or write tty, -2: bad crc
 */
int Modbus_controller::write_reg(uint16_t reg, uint16_t dat)
{
	uint8_t buf[256];
	uint16_t crc;
	int err;
	// try lock, return if timeout
	err = 0;
	while (!err)
		err = this->tty_lock.try_lock_for(std::chrono::milliseconds(5));
	if (!err)
	{
		printf("try lock timeout when write reg, nested call is not allowed!");
		return -4;
	}
	if (this->file_lock->trylock_for(5000) < 0)
	{
		printf("unable to access tty file, semaphore locked by another process");
		this->tty_lock.unlock();
		return -4;
	}

	// if error happened, break and unlock mutex
	do
	{
		buf[0] = this->addr;
		buf[1] = 0x06;
		buf[2] = (reg >> 8) & 0x00FF;
		buf[3] = reg & 0x00FF;
		buf[4] = (dat >> 8) & 0x00FF;
		buf[5] = dat & 0x00FF;
		crc = get_crc(buf, 6);
		// crc low byte first
		buf[6] = crc & 0x00FF;
		buf[7] = (crc >> 8) & 0x00FF;
		err = write(this->fp, buf, 8);
		if (err < 0)
		{
			perror("failed to write tty");
			err = -1;
			break;
		}
		// according to the manual, must delay at least 3.5 characters between two packages,
		// 300us should be enough
		usleep(300);
		err = read(this->fp, buf, 8);
		if (err < 0)
		{
			perror("failed to read tty");
			err = -1;
			break;
		}
		crc = get_crc(buf, 6);
		if (crc != uint8_to_uint16(buf[7], buf[6]))
		{
			err = -2;
			break;
		}
		err = 0;
	} while (0);
	this->file_lock->unlock();
	this->tty_lock.unlock();
	usleep(300);
	return err;
}

/**
 * @brief read device error code register
 *
 * @return int 0: OK, <0: failed to read, >0: error code
 */
int Modbus_controller::read_error(void)
{
	uint8_t buf[2];
	int err;
	err = read_regs(0x00, 1, buf, 2);
	if (err < 0)
	{
		return err;
	}
	return uint8_to_uint16(buf[0], buf[1]);
}

/**
 * @brief read status register
 *
 * @return int <0: failed, 0: idle, 1: laser pointing, 2: measuring
 */
int Modbus_controller::read_status(void)
{
	uint8_t buf[2];
	int err;
	err = read_regs(0x01, 1, buf, 2);
	if (err < 0)
	{
		return err;
	}
	return uint8_to_uint16(buf[0], buf[1]);
}

/**
 * @brief read measure result(0.1mm)
 *
 * @return long <-2000: failed, 0: invalid data, >0: valid data
 */
int Modbus_controller::read_distance(void)
{
	uint8_t buf[4];
	int err;
	err = read_regs(0x02, 2, buf, 4);
	if (err < 0)
	{
		return err - 20000;
	}
	err = uint8_to_int32(buf[0], buf[1], buf[2], buf[3]);
	return err == 0 ? -20000 : err;
}

/**
 * @brief read measure offset value(0.1mm), measure - offset = real
 *
 * @return int 0x7FFFFFFF: failed, other: valid signed int
 */
int Modbus_controller::read_offset(void)
{
	uint8_t buf[2];
	int err;
	int out;
	err = read_regs(0x05, 1, buf, 2);
	if (err < 0)
	{
		return 0x7FFFFFFF;
	}
	return uint8_to_int16(buf[0], buf[1]);
}

/**
 * @brief read device termpreature sensor data(0.1C)
 *
 * @return int 0x7FFFFFFF: failed, other: valid data
 */
int Modbus_controller::read_tempreature(void)
{
	uint8_t buf[2];
	int err;
	err = read_regs(0x08, 1, buf, 2);
	if (err < 0)
	{
		return 0x7FFFFFFF; // burn in hell!
	}
	return uint8_to_int16(buf[0], buf[1]);
}

/**
 * @brief read firmware version(unsigned 16 bits)
 *
 * @return int <0: failed, other: 16bits data
 */
int Modbus_controller::read_version(void)
{
	uint8_t buf[2];
	int err;
	err = read_regs(0x06, 1, buf, 2);
	if (err < 0)
	{
		return err;
	}
	return uint8_to_uint16(buf[0], buf[1]);
}

/**
 * @brief read sample frequency level
 *
 * @return int <0: failed, 0: single, 1: 5Hz, 2: 10Hz, 3: 20Hz, 4: 30Hz
 */
int Modbus_controller::read_frequency(void)
{
	uint8_t buf[2];
	int err;
	err = read_regs(0x07, 1, buf, 2);
	if (err < 0)
	{
		return err;
	}
	return uint8_to_uint16(buf[0], buf[1]);
}

/**
 * @brief read device address register
 *
 * @return int <0: failed, 0: broadcast address, 1-247: valid address, other: invalid
 */
int Modbus_controller::read_address(void)
{
	uint8_t buf[2];
	int err;
	err = read_regs(0x03, 1, buf, 2);
	if (err < 0)
	{
		return err;
	}
	return uint8_to_uint16(buf[0], buf[1]);
}

/**
 * @brief write status register
 *
 * @param status 0: idle, 1: laser pointing 2: measuring
 * @return int <0: failed, 0: success
 */
int Modbus_controller::write_status(uint16_t status)
{
	return write_reg(0x01, status);
}

/**
 * @brief config measure frequency
 *
 * @param level 0: single, 1: 5Hz, 2: 10Hz, 3: 20Hz, 4: 30Hz
 * @return int 0: success, <0: failed
 */
int Modbus_controller::write_frequency(uint16_t level)
{
	return write_reg(0x07, level);
}

/**
 * @brief write offset register
 *
 * @param offset 16 bit signed int(range from -20000 to 20000), in 0.1mm
 * @return int <0: failed 0: success
 */
int Modbus_controller::write_offset(int16_t offset)
{
	return write_reg(0x05, offset);
}

/**
 * @brief write device address register
 *
 * @param address range from 1 to 247, 0 for broaddcast
 * @return int <0: failed, 0: success
 */
int Modbus_controller::write_address(uint16_t address)
{
	return write_reg(0x03, address);
}

/**
 * @brief change remembered address, NOTE: this will not write device address register, to
 * do so, call write_address()
 *
 * @param address address to be set(from 1 to 127), 0 for broadcast
 * @return int address set
 */
int Modbus_controller::set_address(uint16_t address)
{
	this->addr = address;
	return this->addr;
}

/**
 * @brief get remembered address, NOTE: this will not actually read device register, to do
 * so, call read_address()
 *
 * @return int
 */
int Modbus_controller::get_address(void)
{
	return this->addr;
}

/**
 * @brief read unique product id
 *
 * @return long <0: faile, other: product id(low 32 bits)
 */
long Modbus_controller::read_id(void)
{
	uint8_t buf[4];
	int err;
	err = read_regs(0x09, 2, buf, 4);
	if (err < 0)
	{
		return err;
	}
	return uint8_to_uint32(buf[0], buf[1], buf[2], buf[3]);
}

/**
 * @brief write all parameters into internal rom
 *
 * @return int 0: success, <0: failed
 */
int Modbus_controller::save_params(void)
{
	return write_reg(0x18, 0x01);
}

/**
 * @brief tty loopback test
 *
 * @return int some error code
 */
int Modbus_controller::tty_test(void)
{
	uint8_t buf[128];
	int err, i;
	buf[0] = '\0';
	err = write(this->fp, "hello world", 12);
	if (err < 0)
	{
		perror("failed to test tty write");
		return err;
	}
	else
	{
		printf("%d bytes written\r\n", err);
	}
	err = read(this->fp, buf, 128);
	if (err < 0)
	{
		perror("failed to test tty read");
		return err;
	}
	else if (err == 0)
	{
		printf("no data was read\r\n");
	}
	else
	{
		printf("%d bytes read: ", err);
		for (i = 0; i < err; i++)
		{
			printf("%c", buf[i]);
		}
		printf("\r\n");
	}
	return 0;
}
